#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <memory>

// *** 1. 帮助函数：从CSV文件解析位姿 ***
std::vector<geometry_msgs::msg::Pose> parseWaypointsFromCSV(
    const std::string& csv_file_path, 
    rclcpp::Logger logger)
{
    std::vector<geometry_msgs::msg::Pose> waypoints;
    std::ifstream file(csv_file_path);

    if (!file.is_open()) {
        RCLCPP_ERROR(logger, "无法打开CSV文件: %s", csv_file_path.c_str());
        return waypoints; // 返回空列表
    }

    std::string line;
    // (可选) 跳过表头
    // std::getline(file, line); 

    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string value_str;
        std::vector<double> values;

        while (std::getline(ss, value_str, ',')) {
            try {
                values.push_back(std::stod(value_str));
            } catch (const std::invalid_argument& e) {
                RCLCPP_ERROR(logger, "CSV解析错误: '%s' 不是一个有效的数字", value_str.c_str());
                continue; // 跳过这个值
            }
        }

        if (values.size() != 7) {
            RCLCPP_WARN(logger, "CSV行格式错误。应为7个值 (x,y,z,qx,qy,qz,qw)，但只找到 %zu 个。跳过此行。", values.size());
            continue;
        }

        geometry_msgs::msg::Pose pose;
        pose.position.x = values[0];
        pose.position.y = values[1];
        pose.position.z = values[2];
        pose.orientation.x = values[3]; // qx
        pose.orientation.y = values[4]; // qy
        pose.orientation.z = values[5]; // qz
        pose.orientation.w = values[6]; // qw
        waypoints.push_back(pose);
    }

    file.close();
    RCLCPP_INFO(logger, "成功从CSV文件解析了 %zu 个路径点。", waypoints.size());
    return waypoints;
}


// *** 2. 主函数 ***
// -----------------
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    // 为 MoveGroupInterface 创建一个专用的节点
    // MoveGroupInterface 内部有自己的执行器 (spinner)，
    // 但最佳实践是将其放在一个专用的节点中，并显式地为其开辟一个线程。
    auto move_group_node = std::make_shared<rclcpp::Node>(
        "cartesian_path_executor_node",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
    );
    
    // 获取一个日志记录器
    auto logger = move_group_node->get_logger();

    // 在单独的线程中运行节点，以便 MoveGroupInterface 可以处理其回调
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(move_group_node);
    std::thread spinner_thread([&executor]() {
        executor.spin();
    });

    RCLCPP_INFO(logger, "ROS 2 节点和执行器已启动...");

    // 你的 MoveIt 规划组名称
    static const std::string PLANNING_GROUP_NAME = "zc4"; 
    // 你的 CSV 文件路径
    static const std::string CSV_FILE_PATH = "/home/gabo/ros2control/waypoints_path2.csv"; // <--- 修改这里
    // *****************************************

    // --- 2. 设置 MoveGroupInterface ---
    // 使用我们创建的节点来初始化 MoveGroupInterface
    moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP_NAME);
    
    // (可选) 获取一些基本信息
    RCLCPP_INFO(logger, "MoveIt 规划组: %s", move_group.getName().c_str());
    RCLCPP_INFO(logger, "MoveIt 末端执行器: %s", move_group.getEndEffectorLink().c_str());

    // --- 3. (步骤一) 读取和解析CSV文件 ---
    RCLCPP_INFO(logger, "正在从 %s 读取路径点...", CSV_FILE_PATH.c_str());
    std::vector<geometry_msgs::msg::Pose> waypoints = parseWaypointsFromCSV(CSV_FILE_PATH, logger);

    if (waypoints.empty()) {
        RCLCPP_ERROR(logger, "路径点列表为空，程序终止。");
        rclcpp::shutdown();
        spinner_thread.join();
        return -1;
    }


    // --- 4. (步骤二、三、四) 规划笛卡尔路径 ---
    RCLCPP_INFO(logger, "开始计算笛卡尔路径...");
    moveit_msgs::msg::RobotTrajectory trajectory; // 用于存储规划结果
    
    // eef_step: 笛卡尔插值的步长（米）。0.01 表示每 1cm 插值一个点。
    const double eef_step = 0.01; 
    
    // jump_threshold: 关节跳跃阈值（弧度）。设为 0.0 会禁用跳跃检测 (不推荐用于真实硬件)。
    // 设为一个较大的值（如 1.57 或 3.14）来允许一些跳跃，或者设为一个严格的值（如 0.1）来防止跳跃。
    const double jump_threshold = 0.0;

    // `true` 表示需要进行碰撞检测
    bool avoid_collisions = true;

    // 调用 computeCartesianPath
    double fraction = move_group.computeCartesianPath(
        waypoints,
        eef_step,
        jump_threshold,
        trajectory,
        avoid_collisions
    );

    RCLCPP_INFO(logger, "笛卡尔路径规划完成。路径 %.2f%% (共 %zu 个点) 规划成功。", 
              fraction * 100.0, trajectory.joint_trajectory.points.size());


    // --- 5. (步骤五) 检查并执行轨迹 ---
    if (fraction == 1.0) 
    {
        RCLCPP_INFO(logger, "路径规划 100%% 成功！准备执行...");

        // 创建一个 MoveIt 'Plan' 对象
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        my_plan.trajectory = trajectory; // 将计算出的轨迹填充到 Plan 中

        // 执行
        moveit::core::MoveItErrorCode result = move_group.execute(my_plan);

        if (result == moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_INFO(logger, "轨迹执行成功！");
        } else {
            RCLCPP_ERROR(logger, "轨迹执行失败，错误代码: %d", result.val);
        }
    } 
    else 
    {
        RCLCPP_ERROR(logger, "未能规划完整的笛卡尔路径 (仅 %f 成功)。"
                             "可能原因：路径点不可达、IK无解、或路径中存在碰撞。",
                             fraction);
    }

    // --- 6. 清理 ---
    RCLCPP_INFO(logger, "任务完成，关闭节点。");
    rclcpp::shutdown();
    spinner_thread.join(); // 等待执行器线程退出
    return 0;
}